#include "openglcanvas.h"
#include <GL/glu.h>
#include <stdio.h>
#include <string>
#include <iostream>
#include <sstream>
#include "Eigen/Dense"
#include <math.h>
#include <QDebug>
#include <iostream>
#include "glm.hpp"
#include "gtc/matrix_transform.hpp"
#include "gtc/type_ptr.hpp"
#include <QMessageBox>
#include<QDir>
#include<QWheelEvent>
#include "src/globalvar.h"

using namespace std;
OpenglCanvas::OpenglCanvas(QWidget *parent)
    :QGLWidget(parent),
      mPitch(30.0),
      mYaw(0.0),
      mDistance(5),
      QOpenGLFunctions_3_3_Core(),
      mMousePressed(false),
      mLookz(0.0f)
{

}

void OpenglCanvas::SetLookPos(float, float, float xpos)
{
    this->mLookz = xpos;
}


OpenglCanvas::~OpenglCanvas()
{
    glDeleteVertexArrays(1, &mVAO);
    glDeleteBuffers(1, &mVBO);
}


void OpenglCanvas::initializeGL()
{
    this->initializeOpenGLFunctions();
    initializeOpenGLFunctions();

    GLfloat light_position[] = { 0.0, 0.0, 30.0, 0.0};
    GLfloat light_ambient [] = { 0.5, 0.5, 0.0, 1.0};
    GLfloat light_diffuse [] = { 0.8, 0.8, 0.0, 0.5};
    GLfloat light_specular[] = { 0.9, 0.9, 0.0, 0.5};
    glLightfv(GL_LIGHT0, GL_POSITION, light_position);
    glLightfv(GL_LIGHT0, GL_AMBIENT , light_ambient );
    glLightfv(GL_LIGHT0, GL_DIFFUSE , light_diffuse );
    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
    glEnable(GL_LIGHT0);
    glEnable(GL_LIGHTING);

    glEnable(GL_COLOR_MATERIAL);
    glColorMaterial(GL_FRONT,GL_AMBIENT_AND_DIFFUSE);
    glMaterialfv(GL_FRONT,GL_SPECULAR,light_specular);;
    glMateriali(GL_FRONT,GL_SHININESS,80);
    glShadeModel(GL_SMOOTH);
    glFrontFace(GL_CCW);
    glEnable(GL_CULL_FACE);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_NORMALIZE);
    glEnable(GL_AUTO_NORMAL);

    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    // 仿真器偏移器
    mLink1.SetDHPara(0,23.095,-PI/2,0);

    mLink2.SetDHPara(22,0,0,0);
    mLink2.SetThetaRadian(-90.0);


    mLink3.SetDHPara(0,0,PI/2,0);
    mLink3.SetThetaRadian(-90.0);

    mLink4.SetDHPara(0,-24.606,-PI/2,0);
    mLink4.SetTheta(0);

    mLink5.SetDHPara(-16.812,0,PI/2,0);
    mLink5.SetTheta(0);

//    mLink6.SetDHPara(-16.812,0,-PI/2,0);
//    mLink6.SetTheta(0);
    // link all the joint
    mLink1.SetNextModel(&mLink2);
    mLink2.SetNextModel(&mLink3);
    mLink3.SetNextModel(&mLink4);
    mLink4.SetNextModel(&mLink5);
//    mLink5.SetNextModel(&mLink6);

    qDebug()<<mLink4.PosX()<<mLink4.PosY()<<mLink4.PosZ();
    qDebug()<<mLink5.PosX()<<mLink5.PosY()<<mLink5.PosZ();
}

void paintAxis(){
    glBegin(GL_LINES);
    glColor3f(1.0, 0.0, 0.0);
    glVertex3f(0, 0.0f,0);
    glVertex3f(100.0f, 0.0f,0);         // 以上两个点可以画x轴
    glColor3f(0.0, 1.0, 0.0);
    glVertex3f(0.0f, 0,0);
    glVertex3f(0.0f, 100.0f,0);         // 以上两个点可以画y轴
    glColor3f(0.0, 0.0, 1.0);
    glVertex3f(0.0f, 0.0f,0.0f);         // 以上两个点可以画z轴
    glVertex3f(0.0f, 0.0f,100.0f);         // 以上两个点可以画z轴
    glEnd();
}

void OpenglCanvas::paintGL(){
    glPushMatrix();

    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();

    glMatrixMode(GL_PROJECTION);
    gluPerspective(270.0,1.0,10,1000);
    glLoadIdentity();
    glOrtho(-100, +100, -300, +300, -10, 180);
    //this->mLink4.SetView();
    gluLookAt(150,0,0,0,0,this->mLookz + 50,0,0,1);
    glRotatef(mPitch, 0.0, 1.0, 0.0);
    glRotatef(mYaw, 0.0, 0.0, 1.0);
    glScalef(mDistance/5,mDistance/5,mDistance/5);

    glMatrixMode(GL_MODELVIEW);
    paintAxis();
    this->mLink1.ShowStlModelVBO();
    this->mLink2.ShowStlModelVBO();
    this->mLink3.ShowStlModelVBO();
    this->mLink4.ShowStlModelVBO();
    this->mLink5.ShowStlModelVBO();
//    this->mLink6.ShowStlModelVBO();

    glPopMatrix();
}

void OpenglCanvas::resizeGL(int width, int height)
{
    w = width; h = height;
    glViewport(0.0, 0.0, w, h);
    setView();
}

bool OpenglCanvas::event(QEvent *ev)
{
    static bool pressed = false;
    QMouseEvent * p = nullptr;
    static QPoint lastMove;

    switch (ev->type()) {
    case QMouseEvent::MouseButtonPress:
        p = dynamic_cast<QMouseEvent *>(ev);
        lastMove = p->globalPos();
        pressed = true;
        break;
    case QMouseEvent::MouseMove:
        if(pressed){
            p = dynamic_cast<QMouseEvent *>(ev);
            int32_t xmove = p->globalPos().x() - lastMove.x();
            int32_t ymove = p->globalPos().y() - lastMove.y();

            mPitch += float(ymove);
            mYaw += float(xmove);
            lastMove = p->globalPos();
        }
        break;
    case QMouseEvent::MouseButtonRelease:
        pressed = false;
        break;
    case QEvent::Wheel:
        QWheelEvent * p = dynamic_cast<QWheelEvent *>(ev);
        QPoint angle = p->angleDelta();
        if(angle.y() > 0){
            this->mDistance ++;
        }else{
            this->mDistance --;
        }
        break;
    }
    return true;
}


void OpenglCanvas::setPitch(double _pitch)
{
    mPitch = _pitch;
}

void OpenglCanvas::setYaw(double _yaw)
{
    mYaw = _yaw;
}

void OpenglCanvas::setDistance(double _distance)
{
    mDistance = _distance;
}



void OpenglCanvas::OnThetaChangeAngle(float angle1, float angle2, float angle3, float angle4, float angle5, float angle6)
{
    this->mLink1.RotateAxis(angle1);
    this->mLink2.RotateAxis(angle2);
    this->mLink3.RotateAxis(angle3);
    this->mLink4.RotateAxis(angle4);
    this->mLink5.RotateAxis(angle5);
//    this->mLink6.RotateAxis(angle6);

    std::cout<<"link 1 dh"<<std::endl;
    std::cout<<mLink1.DhTransform()<<std::endl;
    std::cout<<"link 2 dh"<<std::endl;
    std::cout<<mLink2.DhTransform()<<std::endl;
    std::cout<<"link 3 dh"<<std::endl;
    std::cout<<mLink3.DhTransform()<<std::endl;
    std::cout<<std::endl;
    std::cout<<"link 4 dh"<<std::endl;
    std::cout<<mLink4.DhTransform()<<std::endl;
    std::cout<<std::endl;
    std::cout<<"link 5 dh"<<std::endl;
    std::cout<<mLink5.DhTransform()<<std::endl;
    std::cout<<std::endl;

    qDebug()<<"link 1 x "<<this->mLink1.PosX()<<" y "<<mLink1.PosY()<<" z "<<mLink1.PosZ()<<" theta "
           <<mLink1.Theta();
    qDebug()<<"link 2 x "<<this->mLink2.PosX()<<" y "<<mLink2.PosY()<<" z "<<mLink2.PosZ()<<" theta "
           <<mLink2.Theta();
    qDebug()<<"link 3 x "<<this->mLink3.PosX()<<" y "<<mLink3.PosY()<<" z "<<mLink3.PosZ()<<" theta "
           <<mLink3.Theta();
    qDebug()<<"link 4 x "<<this->mLink4.PosX()<<" y "<<mLink4.PosY()<<" z "<<mLink4.PosZ()<<" theta "
           <<mLink4.Theta();
    qDebug()<<"link 5 x "<<this->mLink5.PosX()<<" y "<<mLink5.PosY()<<" z "<<mLink5.PosZ()<<" theta "
           <<mLink5.Theta();
//    qDebug()<<"link 5 x "<<this->mLink6.PosX()<<" y "<<mLink6.PosY()<<" z "<<mLink6.PosZ()<<" theta "
//           <<mLink6.Theta();
}

void OpenglCanvas::wheelEvent(QWheelEvent *event)
{
    qDebug()<<event;

}


void OpenglCanvas::setView()
{
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    glRotatef(mPitch, 0.0, -1.0, 0.0);
    glRotatef(mYaw, 0.0, 0.0, -1.0);
    glMatrixMode(GL_MODELVIEW);
}


/*
 * 实现原理见
 * https://www.testingcloud.club/wapi/NewUi/article/103/BCN3D%20MOVEO%E7%9A%84%E9%80%86%E8%BF%90%E5%8A%A8%E5%AD%A6%E6%B1%82%E8%A7%A3
 *
 */
void OpenglCanvas::InverseKinemathic()
{
    double d6_ax = mLink5.DhTransform()(0,0);
    double d6_ay = mLink5.DhTransform()(1,0);
    double d6_az = mLink5.DhTransform()(2,0);



    double d4_pos_x = mLink5.PosX() - d6_ax*mLink5.Twist();
    double d4_pos_y = mLink5.PosY() - d6_ay*mLink5.Twist();
    double d4_pos_z = mLink5.PosZ() - d6_az*mLink5.Twist();

    if(fabs(d4_pos_x) < 0.00001){
        d4_pos_x = 0;
    }
    if(fabs(d4_pos_y) < 0.00001){
        d4_pos_y = 0;
    }
    if(fabs(d4_pos_z) < 0.00001){
        d4_pos_z = 0;
    }

    qDebug()<<d4_pos_x<<d4_pos_y<<d4_pos_z;
    // caculate θ1-3
    if((d4_pos_x == 0)&&(d4_pos_y == 0)){
        qDebug()<<"singular θ1";
    }
    double theta1 = atan2(d4_pos_y,d4_pos_x)/2/PI*360;

    double costheta3 = (d4_pos_x*d4_pos_x + d4_pos_y*d4_pos_y +
                        (d4_pos_z - fabs(mLink1.Distance()))*(d4_pos_z - fabs(mLink1.Distance())) -
                            mLink2.Twist()*mLink2.Twist() - mLink4.Distance()*mLink4.Distance())/2/fabs(mLink2.Twist())/fabs(mLink4.Distance());
    qDebug()<<"costheta3:"<<costheta3;
    double theta3 = (atan2(sqrt(1 - costheta3*costheta3),costheta3) - PI/2)/2/PI*360;
    double theta2 = (PI/2 - atan2(d4_pos_z - mLink1.Distance(),sqrt(d4_pos_x*d4_pos_x + d4_pos_y*d4_pos_y))
                     - (atan2(mLink2.Twist() + mLink4.Distance()*cos(fabs(theta3)/360*2*PI + PI/2),mLink4.Distance()*sin(fabs(theta3)/360*2*PI + PI/2)))
                     )/2/PI*360;
    qDebug()<<"反解θ角度:"<<theta1<<theta2<<theta3;
    mInvTheta.clear();
    mInvTheta.push_back(theta1);
    mInvTheta.push_back(theta2);
    mInvTheta.push_back(theta3);

}

QList<double> OpenglCanvas::InverseTheta()
{
    return mInvTheta;
}




